#ifndef OOPSMP_UTILS_DATASTRUCTURES_CHAINTREE_CHAINTREE_H_
#define OOPSMP_UTILS_DATASTRUCTURES_CHAINTREE_CHAINTREE_H_

#include <iostream>
#include <cstring>
#include <vector>
#include <queue>
#include <assert.h>
#include "OOPSMP/Utils/Geometry/Joints/Joints.h"
#include "OOPSMP/Utils/Geometry/Joints/JointLimits.h"
#include "OOPSMP/Utils/Geometry/Joints/JointsFactory.h"
#include "OOPSMP/Utils/General/BBox/BVGeneric.h"
#include "OOPSMP/Utils/General/BBox/BVFactory.h"
#include "OOPSMP/Utils/Geometry/Workspace/Part.H"

namespace OOPSMP {

	/**
     *@brief Macro for class, class pointer, and const class pointer declarations
     */ 
	ForwardClassDeclaration(CTLink);

   /**
    *@brief chain tree link
    */
	class CTLink {

	public:

		// constructors / destructor

		/* ATTENTION:
		you must either include a bounding box container in the constructor,
		or use setBBoxContainer() during the setup
		*/

		/**
		 *@brief Default constructor for chain tree link
		 */ 
		CTLink( )
			: part( NULL ), isAffected( false ),
			  bbox( NULL ), joint( NULL ),
			  linkDims( NULL ), linkRefPoint( NULL ) { }

		/**
		 *@brief Default constructor for chain tree link
		 *		 used for non-leaf nodes
		 *@param CBBox bbox container for link
		 */
		CTLink( BVGeneric * CBBox )
			: part( NULL ), isAffected( false ),
			  bbox( CBBox ), joint( NULL ),
			  linkDims( NULL ), linkRefPoint( NULL ) { }

		/**
		 *@brief Contructor for chain tree leaf
		 *@param p pointer to part
		 *@param j pointer to joint
		 */
		CTLink( Part_t p, Joints_t j )
			: part(p), joint(j), isAffected( false ),
			  bbox( NULL ), linkDims( NULL ), linkRefPoint( NULL ) { }

		/**
		 *@brief Default destructor
		 */
		~CTLink() { 
			/*if ( linkDims )
				delete [] linkDims;
			if ( linkRefPoint )
				delete [] linkRefPoint;*/
		}

		/**
		 *@brief Returns pointer to link part
		 */
		Part * 		getPart() const		  { return part; }

		/**
		 *@brief Returns true iff link is affected
		 */
		bool		getIsAffected() const { return isAffected; }

		/**
		 *@brief Returns bbox container
		 */
		BVGeneric * getBBox()			  { return bbox; }

		/**
		 *@brief Returns pointer to link joint
		 */
		Joints_t	getJoint() const  	  { return joint; }
		
		// NOTE: following functions currently only work for 3D!!!
		/**
		 *@brief Returns reference point for link
		 *@param linkRef empty array to contain point
		 */
		void		getLinkRefPoint(double * linkRef) const	{
			OOPSMPcopy_vector3( linkRef, linkRefPoint );
		}
		
		/**
		 *@brief Returns length, width, depth of link (in this order)
		 *@param linkDim empty array to contain dimensions
		 */
		void		getLinkDimensions( double * linkDim ) const	{
			OOPSMPcopy_vector3( linkDim, linkDims );
		}

		
		/**
		 *@brief Sets link bbox
		 *@param box array describing new bbox
		 */
		void setBoundingBox( const double * box ) { bbox->setBoundingBoxToArray( box ); }

		/**
		 *@brief Sets if link is affected or not
		 *@param status true iff affected
		 */
		void setIsAffected ( bool status )		  { isAffected = status; }

		/**
		 *@brief Sets bbox container
		 *@param b bbox container
		 */
		void setBBoxContainer( BVGeneric ** b )	  { bbox = *b; }
		
		/**
		 *@brief Sets length, width, depth of link
		 */
		void setLinkDimensions(void)
		{
			if ( linkDims == NULL ) { // only set dimensions once
				linkDims = new double[3];
				bbox->getBBoxDimensions(linkDims);
			}
		}
		
		/**
		 *@brief Sets reference point for link
		 */
		void setLinkRefPoint(void)
		{
			
			if ( linkRefPoint ) // we only want to set the reference point once
				return;

			linkRefPoint = new double [getBBox()->getNumElements() / 2];
				// (double *)calloc( getBBox()->getNumElements() / 2, sizeof(double) );
			double * tmp = new double [ bbox->getNumElements() ];
			bbox->getBBoxAsArray( tmp );

			for ( int i = 0 ; i < getBBox()->getNumElements() / 2 ; i++ )
				linkRefPoint[i] = tmp[i];

			delete [] tmp;
		}

	private:

		/**
		 *@brief pointer to link part
		 */
		Part * part;

		/**
		 *@brief pointer to link joint
		 */
		Joints * joint;

		/**
		 *@brief bbox container
		 */
		BVGeneric * bbox;

		/**
		 *@brief link dimensions
		 */
		double * linkDims;

		/**
		 *@brief link reference point
		 */
		double * linkRefPoint;

		/**
		 *@brief true iff link is affected
		 */
		bool isAffected;

	};

	/**
     *@brief Macro for class, class pointer, and const class pointer declarations
     */ 
	ForwardClassDeclaration(CTNode);
   
   /**
    *@brief chain tree node
    */
	class CTNode {

	public:

		/**
         *@brief Node constructor given link
		 *@param l link
         */
		CTNode( CTLink l )
			: link( l ), childL( -1 ), childR( -1 ), parent( -1 ) { }

		/**
         *@brief Node constructor given children indices
		 *@param left index of left child in chain tree
		 *@param right index of right child in chain tree
         */
		CTNode( int left, int right )
			: link( *(new CTLink()) ), childL( left ), childR( right ),  parent( -1 ) { }

		/**
         *@brief Default node constructor
		 */
		CTNode() {}

		/**
         *@brief Default node destructor
		 */
		~CTNode() {}

		/**
         *@brief Returns reference to link
		 */
		CTLink &getLinkRef()	{ return link; }

		/**
         *@brief Returns copy of link
		 */
		CTLink getLink() const	{ return link; }

		/**
         *@brief Returns index of left child in chain tree
		 */
		int getChildL() const	{ return childL; }

		/**
         *@brief Returns index of right child in chain tree
		 */
		int getChildR() const	{ return childR; }
		
		/**
         *@brief Returns index of parent in chain tree
		 */
		int getParent() const	{ return parent; }

		/**
         *@brief Returns index of node in chain tree
		 */
		int getMyIndex() const	{ return myIndex; }

		/**
         *@brief Returns true iff node is leaf in chain tree
		 */
		bool isLeaf() { return ( childL == -1 && childR == -1 ); }

		/**
         *@brief Sets node index in chain tree
		 *@param id node index
		 */
		void setMyIndex( int id ) { myIndex = id; }

		/**
         *@brief Sets node parent in chain tree
		 *@param id parent index
		 */
		void setParent( int id ) { parent = id; }
		
	private:

		/**
         *@brief Link of node
		 */
		CTLink link;

		/**
         *@brief Node index in chain tree
		 */
		int myIndex;

		/**
         *@brief Left child index in chain tree
		 */
		int childL;

		/**
         *@brief Right child index in chain tree
		 */
		int childR;

		/**
         *@brief Parent index in chain tree
		 */
		int parent;

	};

	/**
     *@brief Macro for class, class pointer, and const class pointer declarations
     */ 
	ForwardClassDeclaration(ChainTree);
	
	/**
    *@brief chain tree
    */
	class ChainTree {

	public:

		/**
         *@brief Default chain tree constructor
		 */
		ChainTree( ) : isComplete( false ), l_MaxLinks( 0 ), l_LinkCount( 0 ) { }

		/**
         *@brief Default chain tree constructor
		 */
		ChainTree( const char * const strBBOX ) : isComplete( false ), l_MaxLinks( 0 ), l_LinkCount( 0 ) { }

		/**
         *@brief Default chain tree destructor
		 */
		~ChainTree() {}

		/**
         *@brief Defines size of chain tree
		 *@param size number of physical links (robot links) to be saved in chain tree
		 */
		void initCTSize( const int size ) { l_MaxLinks = size; v_ChainTree.reserve( size * 2 ); }

		/**
         *@brief Adds physical link to chain tree
		 *@param lnk reference to added link
		 */
		void addLink( CTLink_t lnk ) {

			// use a simple assert to make sure we don't add more links than we declared
			// if we do, it's an invalid XML input, so we should just bail out
			assert( l_LinkCount < l_MaxLinks && !isComplete );

			// wrap the parameter link in a new CTNode and add it to the chain tree
			v_ChainTree.push_back( *(new CTNode(*lnk)) );

			// initialize link's BBox container to the XML input selection
			if ( v_ChainTree.back().getLink().getBBox() == NULL ) {
				BVGeneric_t bv = BVFactory::getBBoxInstance( strBBoxType );
				v_ChainTree.back().getLinkRef().setBBoxContainer( &bv );
			}
			// increment link count for the chain tree
			l_LinkCount += 1;
		}

		/**
         *@brief Returns reference to node in chain tree; zero based
		 *@param index node index in chain tree
		 */
		CTNode &getNodeRef( int index ) { return v_ChainTree.at(index); }

		/**
         *@brief Returns constant pointer to node in chain tree; zero based
		 *@param index node index in chain tree
		 */
		const CTNode &getNode( int index ) const { return v_ChainTree.at(index); }

		/**
         *@brief Returns index of chain tree root
		 */
		int getRootId() const { return v_ChainTree.size()-1; }

		/**
         *@brief Returns reference to chain tree root
		 */
		CTNode &getRootRef() { return v_ChainTree.back(); }

		/**
         *@brief Returns copy of chain tree root
		 */
		CTNode getRoot() const { return v_ChainTree.back(); }

		/**
         *@brief Computes bounding box of node in chain tree
		 *@param index nodex index in chain tree
		 */
		void computeBoundingBox( int index ) {

			// get the node
			CTNode node = getNodeRef( index );

			// if the node isLeaf
			if ( node.isLeaf() ) {
				node.getLinkRef().getBBox()->calculateBoundingBox( node.getLink().getPart() );
			}
			// otherwise, compute BB from BBs of the node's two children
			else {		
				// get the children
				CTNode ch_L = getNode( node.getChildL() );
				CTNode ch_R = getNode( node.getChildR() );

				// get their bounding boxes
				double * bboxL = new double[ch_L.getLink().getBBox()->getNumElements()];
				ch_L.getLink().getBBox()->getBBoxAsArray( bboxL );
				double * bboxR = new double[ch_R.getLink().getBBox()->getNumElements()];
				ch_R.getLink().getBBox()->getBBoxAsArray( bboxR );

				// create a new vector containing bbox vertices from both children
				std::vector<double> bboxes;
				int length = ch_L.getLink().getBBox()->getNumElements();

				// we only want the first 6 elements of each bbox - that is, XYZ of each point (4 total) 
				for ( int i = 0 ; i < 6 ; i++ ) bboxes.push_back( bboxL[i] );
				for ( int i = 0 ; i < 6 ; i++ ) bboxes.push_back( bboxR[i] );

				// call link's bbox's calculateBoundingBox, which updates the internals of the BBox
				double * bboxesArray = new double[bboxes.size()];
				for ( unsigned i = 0 ; i < bboxes.size() ; i++ ) { bboxesArray[i] = bboxes.at(i); }
				// node.getLinkRef().getBBox()->calculateBoundingBox( vectorToArray( bboxes ), 4 );
				node.getLinkRef().getBBox()->calculateBoundingBox( bboxesArray, 4 );

				delete [] bboxesArray;
				delete [] bboxR;
				delete [] bboxL;

			}

		}

		/**
         *@brief Builds chain tree for the provided links
		 */
		void completeChainTree() {
			// make sure we have all declared links and that the tree has not been built yet
			assert( !isComplete && l_MaxLinks == l_LinkCount );

			// run over all links and set their indexes to corresponding positions in array
			int currentIndex = 0;
			for ( currentIndex = 0 ; currentIndex < l_LinkCount ; currentIndex++ )
				getNodeRef(currentIndex).setMyIndex( currentIndex );

			// declare two queues for processing the tree
			std::queue< CTNode > workingQueue;
			std::queue< CTNode > nextQueue;

			// populate the working queue with all the links
			for ( int i = 0 ; i < l_LinkCount ; i++ )
				workingQueue.push( v_ChainTree[i] );

			// continue until all nodes have been processed
			while ( !workingQueue.empty() ) {

				// while there are still pairs left in the queue
				while( workingQueue.size() > 1 ) {

					// pop the first two nodes from the queue
					CTNode nodeL (workingQueue.front()); workingQueue.pop();
					CTNode nodeR (workingQueue.front()); workingQueue.pop();
					CTNode newNode( nodeL.getMyIndex(), nodeR.getMyIndex() );

					BVGeneric * bv = BVFactory::getBBoxInstance( strBBoxType );
					newNode.getLinkRef().setBBoxContainer( &bv );
					newNode.setMyIndex( currentIndex++ );

					// update the two children with the new parent
					getNodeRef( nodeL.getMyIndex() ).setParent( newNode.getMyIndex() );
					getNodeRef( nodeR.getMyIndex() ).setParent( newNode.getMyIndex() );

					// add the new node to the next processing cycle
					nextQueue.push( newNode );

					// save the node
					v_ChainTree.push_back( newNode );

				}

				// if there was an odd number of items, add the last (non-paired) item to the next cycle
				if ( workingQueue.size() ) {
					nextQueue.push( workingQueue.front() ); workingQueue.pop();
				}

				// if the next cycle contains only one node, we've reached the root and are done.
				if ( nextQueue.size() == 1 ) {
					// v_ChainTree.push_back( nextQueue.front() );
					nextQueue.pop();
				}
				// otherwise, copy the nextQueue into workingQueue and then empty the nextQueue
				else {
					workingQueue = nextQueue;
					while ( nextQueue.size() ) nextQueue.pop();
				}

			}

			// compute bounding boxes for all nodes in the tree
			for ( unsigned int i = 0 ; i < v_ChainTree.size() ; i++ )
				computeBoundingBox( i );

			// compute link dimensions and reference points for all physical links
			for ( int i = 0 ; i < l_LinkCount ; i++ ) {
				getNodeRef(i).getLinkRef().setLinkDimensions();
				getNodeRef(i).getLinkRef().setLinkRefPoint();
			}

			// set isComplete flag for the tree to prevent further changes
			isComplete = true;
		}

		/**
         *@brief Propagates affected status up the tree
		 *@param index node index to propagate from
		 */
		void setIsAffected( int index ) {

			do {
				computeBoundingBox( index );
				getNodeRef( index ).getLinkRef().setIsAffected( true );
				index = getNodeRef( index ).getParent();
			} while ( index != -1 );

		}

		/**
         *@brief Sets bbox class used for this tree
		 *@param strType name of bbox class
		 */
		void setBBoxType( const char * const strType ) {
			strcpy(strBBoxType, strType);
		}

		/**
         *@brief Returns number of physical links in this tree 
		 */
		int getLinkCount() const {
			return l_LinkCount;
		}

	private:

		/**
         *@brief vector containing all of the chain tree nodes
		 */
		std::vector< CTNode > v_ChainTree;

		/**
         *@brief name of the bounding box class used for this tree
		 */
		char strBBoxType[20];
		
		/**
         *@brief maximum expected snake length
		 */
		int l_MaxLinks;

		/**
         *@brief number of link already added
		 */
		int l_LinkCount;	

		/**
         *@brief root of the index node
		 */
		int l_rootIndex;

		/**
         *@brief flag to mark the ChainTree completion
		 */
		bool isComplete; 

	};

}

#endif
